/*
 * lane_model_fitting.h
 *
 *  Created on: May 11, 2018
 *      Author: geoff
 */
#ifndef RMD_LANE_MODEL_FITTING_H_
#define RMD_LANE_MODEL_FITTING_H_

#include "data_frame.h"
#include "lane_kalman.h"
#include "lane_line_struct.h"

namespace rmd {
class LaneModelFitting {
 private:
   DataFrame* data_frame_;
  cv::Mat show_hough_image_;
  cv::Mat show_cluster_image_;
  cv::Mat show_origin_image_;

  cv::Mat pm_gray_image;
  cv::Mat pm_black_image;

  std::vector<KeyLine> pri_near_lines_;
  std::vector<KeyLine> pri_middle_lines_;
  std::vector<LaneLine> pri_lane_lines;

  long lindex;

  short image_width;
  short image_height;

  short end_line_;
  short middle_start_line_;
  short far_line_;
  short start_line_;

  cv::Point adapt_vanishing_point;
  cv::Point constant_vanishing_point;

  std::vector<float> lane_line_fitting_width_;

 private:
  void SetDataFrameToClass(DataFrame* date_fream, cv::Mat& pm_blac);
  std::vector<cv::Vec4i> GetFieldLinesV0(int start_rows, int end_rows);
  std::vector<KeyLine> FrameAssociation(std::vector<KeyLine>& cur_keylines,
                                        int nmin, int nmax, int nindex);
  std::vector<LaneLine> NearMiddleFieldCombine(
      std::vector<KeyLine>& near_lanelines,
      std::vector<KeyLine>& middle_lanelines);

  std::vector<KeyLine> ClusterLine(std::vector<cv::Vec4i>& lines);
  std::vector<KeyLine> AutoClusterLine(std::vector<cv::Vec4i>& lines);

  void ClusterLineSubmodule(std::vector<KeyLine>& keylines_result);

  void ExcludeOutLiersLines(std::vector<KeyLine>& key_lines);

  void TestVanishingPoint(std::vector<KeyLine>& key_lines,bool bflag = true);
  void TestFrameAssoctation();
  void TestNearMiddleFieldCombine(std::vector<LaneLine>& line_results);
  void TestEkfResult(std::vector<LaneLine>& ekf_line_results,
                     std::vector<LaneLine>& line_results);

  void SetAutoLdVps(cv::Point left_up, cv::Point left_down,
                    cv::Point right_up, cv::Point right_down);
 public:
  LaneModelFitting();
  virtual ~LaneModelFitting();
   CKalman* lane_kalman_;
  std::vector<LaneLine> Update(DataFrame* date_fream, cv::Mat& pm_black);
  void UpdateAuto(DataFrame* data_frame, cv::Mat& pm_black);
  long GetIindex();
};
}

#endif
